//
// File:	AIWisdom.cpp
//
// Purpose:	Test program for FFLL API
//
// Copyright � 2001 Louder Than A Bomb! Software
//
//
//

#include "Aria.h"
#include "ipc.h"


#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc_c.h"

#include <iostream>
#include <vector>
#include <stdio.h>

#include <cstdio>
#include <cstdlib>
#include <iostream>	// for i/o functions

#include "FFLLAPI.h"	// FFLL API

#define TARGET	0 // target is 1st variable
#define OBSTACLES	1 // obstacles is 2nd variable

const int SONAR_NUM = 8;

const double PI = 3.1415926535897932384626433832795;

const double OBS_DIST_THRES = 1000; // mm

const double H_LIN_VEL = 30; // mm/s
const double N_LIN_VEL = 20;
const double L_LIN_VEL = 10;

const double H_ROT_VEL = PI; // rad/s
const double N_ROT_VEL = PI/2;
const double L_ROT_VEL = PI/4;

using namespace std;


Mat target_stack;
vector<Point2f> target_stack_vect;


float target_positioner()
{
	char *centralhost="140.118.38.92";
	double pose[3];


	myIPC_Init(centralhost);
	Sleep(200);

	// get robot heading
	myGetRobotPose(pose);
	Sleep(200);
	printf("%f  %f  %f \n", pose[0], pose[1], pose[2]);

	// get delta angle
	// where's the function


	// do the minus

	myShutdown();	
	system("pause");

	return 1;
}

float obs_positioner()
{
	Mat rb_sonars = Mat::zeros(1, SONAR_NUM, CV_64F);

	// Check all sonars
	int val;
	for (int i=0; i<SONAR_NUM; i++)
	{	 
		val = getSonarRange(i);
		rb_sonars.at<double>(i) = val;

		// If sonar i < OBS_DIST_THRES, it's an obstacle
		// what if obstacle is big, blocking several sonars ??
		if (val < OBS_DIST_THRES)
		{
			flag = 1;
		}
	}

	// return value;
	return 1;	
}

void steering()
{
	// perform steering
	// turn left 

	// turn LF 

	// straight 

	// turn RF 

	// turn right 

}

void speedor()
{
	// perform speed control
	
}

void controller()
{

	// go straight to target
	

	// store a stack of targets


	// create new target if current target is blocked

}

void fuzzyController()
{
	cout.setf(ios::fixed);
	cout.precision(2); // only display 2 decimal places

	// create and load the model
	int model = ffll_new_model();


	//int ret_val = (int)ffll_load_fcl_file(model, "..\\aiwisdom.fcl");
	int ret_val = (int)ffll_load_fcl_file(model, "fuzzycontroller.fcl");
	//int ret_val = (int)ffll_load_fcl_file(model, "aiwisdom.fcl");

	if (ret_val < 0)
	{
		cout << "Error Opening fuzzycontroller.fcl";
		getchar();
		// make sure the "working directory" in "Project | Settings..."
		// is set to the executable's directory if running from the MSVC IDE
		return 0;
	}

	// create a child for the model...
	int child = ffll_new_child(model);

	while (1)
	{
		cout << "SELECT AN OPTION:\n\tS - set values\n\tQ - quit";
		cout << endl;
		cin >> option;

		if (option == 'Q' || option == 'q')
			break;

		if (option == 'S' || option == 's')
		{
			cout << "Target: " ;
			cin >> target_pos;
			cout << "Obstacles: "  ;
			cin >> obstacles_pos;

			// set input variables...

			ffll_set_value(model, child, TARGET, target_pos);
			ffll_set_value(model, child, OBSTACLES, obstacles_pos);

			// get and display the output value
			int output = (int)ffll_get_output_value(model, child);  // what's output? if having 2 values?

			cout << "output: ";

			cout << output;

			switch(output)  // how to work if there are 2 values??
			{
			case (1):
				cout << "Run Away!";
				break;

			case (2):
				cout << "Fight Defensively";
				break;

			case (3):

				cout << "All Out Attack!";
				break;

			} // end switch

			cout << endl;
		} // end if option = 's'

	} // end while(1)
}

int main(int argc, char* argv[])
{
    float	target_pos, obstacles_pos; // values for input variables
    char	option;	// var for selection of what user wants to do

	//target_stack = Mat::zeros() -> use a  dynamic vector


	while (1)
	{

		// get obstacle status


		// get target status


		// get mode: virtual target OR real target

		
		// feed to fuzzy controller to have output decision 


		// execute decision

		char c = getchar();
		if (c == 65)
		{
			break;
		}
	}  

    return 0;

} // end main()


